#include "algo_debris.h"
#include "libdebris/LargeDebrisDetector.h"

#include <stdlib.h>

#include "util.h"
#include "adlist.h"
#include "modbus-srv.h"

#define DS_NORMAL 0
#define DS_STOP 1
#define DS_WARN 2
#define DS_ALARM 4
#define DS_IRONCLEAN 8

struct MT_LARGEDEVRIS_PARAM* debris_param = NULL;
list* debris_list = NULL;
const int debris_list_len = 60;

int init_debris(void* algo)
{
    debris_param = (struct MT_LARGEDEVRIS_PARAM*)malloc(sizeof(struct MT_LARGEDEVRIS_PARAM));
    if (!debris_param) {
        __log_mesg("malloc error.\n");
        return 1;
    }
    debris_param->DetectionROI.x = 720;
    debris_param->DetectionROI.y = 400;
    debris_param->DetectionROI.w = 720;
    debris_param->DetectionROI.h = 450;

    debris_param->CTQROI.x = 1100;
    debris_param->CTQROI.y = 200;
    debris_param->CTQROI.w = 500;
    debris_param->CTQROI.h = 200;

    debris_param->CTQ_detection_sens = 50;
    debris_param->foreground_detection_sys = 50;
    debris_param->frame_rate = 20;
    debris_param->early_warning_sens = 160;
    debris_param->warning_sens = 160;
    debris_param->startbackgroundUpdateTime = 20;
    debris_param->stopbackgroundUpdateTime = 1;
    MT_LargeDebris_Alg_Init(*debris_param);

    debris_list = listCreate();
    listSetFreeMethod(debris_list, free); 
    return 0;
}

void* compute_debris(void* algo, unsigned char* data, int width, int height)
{
    struct MT_LARGEDEVRIS_RESULT* res = (struct MT_LARGEDEVRIS_RESULT*)malloc(sizeof(struct MT_LARGEDEVRIS_RESULT));
    *res = MT_LargeDebris_Alg_GetFrameResult(data, width, height);
    return res;
}

int _analysis(list* debrislist)
{
    int stopcnt = 0, warncnt = 0, alarmcnt = 0, ironclean_cnt = 0; 
    listIter *iter = listGetIterator(debrislist, AL_START_HEAD);
    listNode* node = NULL;
    while((node = listNext(iter))) {
        struct MT_LARGEDEVRIS_RESULT* res = (struct MT_LARGEDEVRIS_RESULT*)node->value;
        if (res->result == DS_NORMAL) continue;
        switch (res->result)
        {
        case DS_STOP:
            stopcnt++;
            break;
        case DS_WARN:
            warncnt++;
            break;
        case DS_ALARM:
            alarmcnt++;
            break;
        case DS_IRONCLEAN:
            ironclean_cnt++;
            break;
        default:
            __log_mesg("Unkown debris result(%d)\n", res->result);
            break;
        }
    }
    if (stopcnt == debris_list_len) return DS_STOP;
    if (warncnt > 30) return DS_WARN; 
    if (alarmcnt > 50) return DS_ALARM;
    if (ironclean_cnt == debris_list_len) return DS_IRONCLEAN;
    return DS_NORMAL;
}

//void debug_debris(unsigned char* data, int width, int height);
void* analysis_debris(void* input)
{
    int state = 0;
    listAddNodeTail(debris_list, input);
    if (listLength(debris_list) < debris_list_len) {
        return state;
    }

    return _analysis(debris_list);    

}

int response_debris(void* algo, void* input)
{
    static int prev_sts = DS_NORMAL;
    int sts = (int)input;

    if (sts != prev_sts) {
        __log_mesg("Debris detect result = %d.\n", sts);
        set_input_register_uint16(sts, TIE_DEBRIS_STATUS_ADDR);
    } 
    prev_sts = sts;
}

void destroy_debris(void* algo)
{
    if (debris_param) {
        free(debris_param);
        debris_param = 0;
    }

    if (debris_list) {
        listRelease(debris_list);
    }
    
    MT_LargeDebris_Alg_Release();
}